#include "i2c.h"

#if USE_I2C

#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "../../board.h"

/* APBC Clock Control bits */
#define SW_RESET                (1 << 2)
#define CLOCK_ENABLE            (1 << 1)
#define BUS_CLOCK_ENABLE        (1 << 0)

#define CR_FIFO_CTRL_BITS       (CR_START | CR_STOP | CR_ACKNAK | CR_TB) /* FIFO Control Bits */

#define I2C_FLAG_RD             (1 << 0)                /* read data, from slave to master */
#define I2C_FLAG_WR             (1 << 1)                /* Write to Slave */
#define I2C_FLAG_STOP           (1 << 2)                /* send STOP after complete this msg */
#define I2C_FLAG_NO_ADDR        (1 << 3)                /* Do not send addr */

#define I2C_MSG_STATE_READY     (0)
#define I2C_MSG_STATE_START     (1)
#define I2C_MSG_STATE_RECEIVE   (2)
#define I2C_MSG_STATE_STOP      (3)

#define TX_FIFO_SIZE            (8)
#define RX_FIFO_SIZE            (16)
#define RFIFO_PTR_MAX           (32)

/* I2C Wait Complete Flag */
#define I2C_XFER_DONE           (1 << 0)
#define I2C_XFER_ERR            (1 << 1)
#define I2C_XFER_TIMEOUT        (1 << 2)

/* Loop Error Detect */
#define TX_FIFO_LOOP_MAX        (50)
#define RX_FIFO_LOOP_MAX        (50)

/* Error Msg Buf Len */
#define I2C_LOG_BUF_LEN         (128 * 1)

struct i2c_msg {
    unsigned addr;                  /* slave address */
    unsigned flags;                 /* I2C_FLAG_xxx */
    int len;                        /* buf length */
    unsigned char *buf;             /* data for tx */
    unsigned state;                 /* I2C_MSG_STATE_xxx */
    struct i2c_msg *next;
    int tx_idx;                     /* index of buf for tx */
    int rx_idx;                     /* index of buf for rx */
};

struct i2c_master_t {
    unsigned master_reg;
    unsigned clk_reg;
    int pinmux_idx;
    unsigned speed;
    struct i2c_msg *msgs;           /* msg array */
    struct i2c_msg *curr_msg;       /* current handling msg */
    unsigned irq;
    unsigned error;
    char *log_buf;
    int log_buf_used;
    unsigned virq;
    unsigned xfer_timeout;          /* wait complete timeout (ms) */
#if USE_SDK_API
    uint8_t xfer_done;              /* uos wait flag */
    uint8_t mutex;
    uint8_t xfer_mutex;
#else
    volatile uint8_t xfer_done;
#endif
};

typedef union {
    struct {
        unsigned data : 8;          /* Data */
        unsigned icr_start : 1;     /* ICR Start */
        unsigned icr_stop : 1;      /* ICR Stop */
        unsigned icr_acknak : 1;    /* ICR ACKNAK */
        unsigned icr_tb : 1;        /* ICR Transfer Byte */
        unsigned reserved : 20;     /* Unused */
    } s;
    unsigned value;
} fifo_entry_t;

static struct i2c_master_t i2c_master[I2C_MASTER_MAX] = {
    {   /* CI2C 0 */
        .master_reg =  TWSI0_BASE,
        .clk_reg = APBC_TWSI0_CLK_RST,
        .pinmux_idx = 0,
        .irq = 7,
    },
    {   /* CI2C 1 */
        .master_reg =  TWSI1_BASE,
        .clk_reg = APBC_TWSI1_CLK_RST,
        .pinmux_idx = 1,
        .irq = 15,
    },
    {   /* PI2C */
        .master_reg =  TWSI2_BASE,
        .clk_reg = APBC_TWSI2_CLK_RST,
        .pinmux_idx = -1,   /* unconfigable */
        .irq = 54,
    },
    {   /* CI2C 2 */
        .master_reg =  TWSI3_BASE,
        .clk_reg = APBC_TWSI3_CLK_RST,
        .pinmux_idx = 2,
        .irq = 93,
    },
    {   /* CI2C 3 */
        .master_reg =  TWSI4_BASE,
        .clk_reg = APBC_TWSI4_CLK_RST,
        .pinmux_idx = 3,
        .irq = 98,
    },
};

static const struct i2c_conf_t i2c_conf[I2C_MASTER_MAX] = {
    { .speed = I2C_SPEED_STANDARD },    /* CI2C0 */
    { .speed = I2C_SPEED_FAST },        /* CI2C1 */
    { .speed = I2C_SPEED_FAST },        /* PI2C  */
    { .speed = I2C_SPEED_FAST },        /* CI2C2 */
    { .speed = I2C_SPEED_FAST },        /* CI2C3 */
};

static void print_log_buf(struct i2c_master_t *pdev)
{
    char *p, *end;

    p = pdev->log_buf;
    while (1) {
        end = strchr(p, '\n');
        if (!end) {
            printf("%s\n", p);
            break;
        }
        *end = '\0';
        printf("%s\n", p);
        p = end + 1;
    }

    return;
}

#define BUF_OVERRUN_STR "\nLog Buf Overrun"
static int i2c_log_to_buf(struct i2c_master_t *pdev, const char *fmt, ...)
{
    char *p;
    int n, ret;
    va_list ap;
    char tmp[24];

    n = I2C_LOG_BUF_LEN - strlen(pdev->log_buf);
    n = (n <= 1)?1:n;

    p = strchr(pdev->log_buf, '\0');
    va_start(ap, fmt);
    ret = vsnprintf(p, n, fmt, ap);
    va_end(ap);
    pdev->log_buf_used += ret;

    if (ret > (n - 1)) { /* vsnprintf truncated */
        sprintf(tmp, " %d bytes used\n", pdev->log_buf_used);
        p = pdev->log_buf + (I2C_LOG_BUF_LEN - 1) - strlen(BUF_OVERRUN_STR) - strlen(tmp) - 1;
        sprintf(p, "%s%s", BUF_OVERRUN_STR, tmp);
    }

    pdev->log_buf[I2C_LOG_BUF_LEN - 1] = '\0';

    return 0;
}

static struct i2c_master_t *get_i2c_master(i2c_t dev)
{
    if (dev < I2C_MASTER_MAX) {
        return (i2c_master + dev);
    }
    else {
        printf("Invalid i2c master id, get_i2c_master failed\n");
        return NULL;
    }
}

#if USE_SDK_API
static struct i2c_master_t *virq_to_pdev(unsigned virq)
{
    for (int i = 0; i < I2C_MASTER_MAX; i++) {
        if ((i2c_master + i)->virq == virq) {
            return (i2c_master + i);
        }
    }
    printf("virq_to_pdev(%u) failed\n", virq);
    return NULL;
}
#endif

#define I2C_XFER_TIMEOUT_MIN   (200) /* 200 ms */
#define SPEED_TO_BYTE(X)       ((X) * 1000 / 10)

static int calc_timeout(struct i2c_master_t *pdev)
{
    int speed = 100, len = 0;
    struct i2c_msg *m = pdev->msgs;
    unsigned timeout_ms;

    while (m) {
        len += m->len;
        m = m->next;
    }

    if (pdev->speed == I2C_SPEED_STANDARD) {
        speed = 100;
    } else if (pdev->speed == I2C_SPEED_FAST) {
        speed = 400;
    } else if (pdev->speed == I2C_SPEED_HIGH) {
        speed = 3400;
    }

    timeout_ms = (len * 1000) / SPEED_TO_BYTE(speed);
    if (timeout_ms < I2C_XFER_TIMEOUT_MIN) {
        timeout_ms = I2C_XFER_TIMEOUT_MIN;
    }

    pdev->xfer_timeout = timeout_ms;
    //printf("speed %d, len %d, timeout %u(ms)\n", speed, len, pdev->xfer_timeout);
    return len;
}

static int i2c_lock_bus(struct i2c_master_t *pdev)
{
#if USE_SDK_API
    uos_take_mutex(pdev->xfer_mutex);
#endif

    return 0;
}

static int i2c_unlock_bus(struct i2c_master_t *pdev)
{
#if USE_SDK_API
    uos_release_mutex(pdev->xfer_mutex);
#endif

    return 0;
}

static void i2c_xfer_ready(struct i2c_master_t *pdev)
{
    calc_timeout(pdev);
    pdev->log_buf[0] = '\0';
    pdev->log_buf_used = 1; /* string terminating null byte '\0' */
}

static void _i2c_clk_enable(struct i2c_master_t *pdev, int enable)
{
    unsigned val;

    val = enable ? (CLOCK_ENABLE | BUS_CLOCK_ENABLE) : (SW_RESET); /* apb function clk 0:33mhz */
    writel(val, APBC_BASE + pdev->clk_reg);
}

static void i2c_set_mode(struct i2c_master_t *pdev)
{
    uint32_t LCR = 0,SLV = 0,FLV = 0,HLVL = 0,HLVH = 0,WCR,WCR_COUNT;
    uint32_t fclk,I2c_clk;
    int mode;

    if (!pdev) {
        goto error;
    }
    mode = pdev->speed;

    fclk = 33000;//33mhz = 33000khz
    LCR = readl(pdev->master_reg + TWSI_LCR);
    SLV = LCR & 0x1ff;
    FLV = (LCR & 0x3fe00) >> 9;
    HLVH = (LCR & 0x7fc0000) >> 18;
    HLVL = (LCR & 0xf8000000) >> 27;
    switch(mode){
        case I2C_SPEED_STANDARD:
            I2c_clk = 100;//i2c clk 100khz
            SLV = (fclk / I2c_clk - 8) / 2;
            SLV &= 0x1ff;
        break;
        case I2C_SPEED_FAST:
            I2c_clk = 400;//i2c clk 400khz
            FLV = (fclk / I2c_clk - 8) / 2 - 1;
            FLV &= 0x1ff;
            WCR = readl(pdev->master_reg + TWSI_WCR);
            WCR_COUNT = WCR & 0x1f;
            if(WCR_COUNT > FLV / 3)
            {
                WCR_COUNT = FLV / 3;
                writel((WCR & (~0x1f)) | WCR_COUNT, pdev->master_reg + TWSI_WCR);
            }
        break;
        case I2C_SPEED_HIGH:
            I2c_clk = 1800;//i2c clk 1.8Mhz
            HLVH = 6;  //use register default value
            HLVL = fclk / I2c_clk - 9 - HLVH;
            HLVL &= 0x1f;
        break;
        default:
           printf("i2c_set_mode failed: not support mode:%d\n",mode);
           break;
    }
    writel((HLVL << 27) | (HLVH << 18) | (FLV << 9) | SLV, pdev->master_reg + TWSI_LCR);
    return;
error:
    printf("i2c_set_mode failed\n");
    return;
}

static void lowlevel_init(struct i2c_master_t *pdev)
{
    uint32_t cr = 0;

    writel(0, pdev->master_reg + TWSI_CR); /* Disable SCL and Unit */

    i2c_set_mode(pdev);

    cr |= 1 << 16; /* HS bit (CR[16]) always set according to Datasheet */
    if (pdev->speed == I2C_SPEED_FAST) {
        cr |= 1 << 15;
    }

    writel(cr, pdev->master_reg + TWSI_CR);
}

static void i2c_dev_reset(struct i2c_master_t *pdev)
{
    printf("i2c dev reset\n");
    _i2c_clk_enable(pdev, 0);
    uos_sleep(2); /* 2 * 5 ms */
    _i2c_clk_enable(pdev, 1);
    lowlevel_init(pdev);
}

static void reset_fifo(struct i2c_master_t *pdev)
{
    writel(0, pdev->master_reg + TWSI_WFIFO_RPTR);
    writel(0, pdev->master_reg + TWSI_WFIFO_WPTR);
    writel(0, pdev->master_reg + TWSI_RFIFO_RPTR);
    writel(0, pdev->master_reg + TWSI_RFIFO_WPTR);
}

static void unit_reset(struct i2c_master_t *pdev)
{
    writel(CR_UR, pdev->master_reg + TWSI_CR);
    writel(0, pdev->master_reg + TWSI_CR);
    lowlevel_init(pdev);
    reset_fifo(pdev);
}

static void fifo_xfer_enable(struct i2c_master_t *pdev, int enable)
{
    uint32_t cr;

    /* Stop and then Clear */
    cr = readl(pdev->master_reg + TWSI_CR);
    cr &= ~(CR_FIFOEN | CR_TXDONE_IE | CR_TXE_IE | CR_RXHF_IE | CR_RXOV_IE | CR_TXBEGIN | CR_IUE | CR_SCLE);
    writel(cr, pdev->master_reg + TWSI_CR);
    cr &= ~CR_FIFO_CTRL_BITS;
    writel(cr, pdev->master_reg + TWSI_CR);

    reset_fifo(pdev);
    writel(readl(pdev->master_reg + TWSI_SR), pdev->master_reg + TWSI_SR);

    if (enable) {
        cr |= CR_FIFOEN | CR_TXDONE_IE | CR_TXE_IE | CR_RXHF_IE | CR_RXOV_IE | CR_TXBEGIN | CR_IUE | CR_SCLE;
        writel(cr, pdev->master_reg + TWSI_CR);
    }

    //printf("fifo_xfer_enable(%d), cr = 0x%x/0x%x\n", enable, (unsigned)cr, (unsigned)readl(pdev->master_reg + TWSI_CR));
}

#if USE_SDK_API
static void i2c_xfer_done(struct i2c_master_t *pdev)
{
    uos_set_flag(pdev->xfer_done, I2C_XFER_DONE, UOS_FLAG_OR);
}

static void i2c_xfer_err(struct i2c_master_t *pdev)
{
    uos_set_flag(pdev->xfer_done, I2C_XFER_ERR, UOS_FLAG_OR);
}

static int i2c_wait_complete(struct i2c_master_t *pdev)
{
    uint32_t flag = 0;
    unsigned os_tick;

    os_tick = (pdev->xfer_timeout / 5) & UOS_SUSPEND; /* Max value is UOS_SUSPEND */
    uos_wait_flag(pdev->xfer_done, I2C_XFER_DONE | I2C_XFER_ERR, UOS_FLAG_OR_CLEAR, &flag, os_tick);
    if (flag == 0) {/* timeout */
        printf("i2c timeout(%d ms)\n", pdev->xfer_timeout);
        unit_reset(pdev);
        print_log_buf(pdev);
    }

    if (flag & I2C_XFER_ERR) {
        printf("i2c err: 0x%x\n", pdev->error);
        unit_reset(pdev);
        print_log_buf(pdev);
    }

    return (flag & I2C_XFER_DONE)?0:-1;
}
#else
#include "xtimer.h"

static void timeout_cb(void * arg)
{
    struct i2c_master_t *pdev = (struct i2c_master_t *)arg;
    pdev->xfer_done = I2C_XFER_TIMEOUT;
}

static xtimer_t tmr = {
    .callback = timeout_cb,
};

static void i2c_xfer_done(struct i2c_master_t *pdev)
{
    pdev->xfer_done = I2C_XFER_DONE;
}

static void i2c_xfer_err(struct i2c_master_t *pdev)
{
    pdev->xfer_done = I2C_XFER_ERR;
}

static int i2c_wait_complete(struct i2c_master_t *pdev)
{
    int ret;

    tmr.arg = pdev;
    xtimer_set(&tmr, US_PER_MS * pdev->xfer_timeout);

    while (pdev->xfer_done == 0) {
        ;
    }

    xtimer_remove(&tmr);
    if (pdev->xfer_done == I2C_XFER_TIMEOUT) {
        printf("i2c timeout(%d ms)\n", pdev->xfer_timeout);
        unit_reset(pdev);
        print_log_buf(pdev);
    }

    if (pdev->xfer_done == I2C_XFER_ERR) {
        printf("i2c err: 0x%x\n", pdev->error);
        unit_reset(pdev);
        print_log_buf(pdev);
    }

    ret = (pdev->xfer_done == I2C_XFER_DONE)?0:-1;
    pdev->xfer_done = 0;

    return ret;
}

#endif

static struct i2c_msg *create_msg_list(struct i2c_master_t *pdev, int num)
{
    struct i2c_msg *msg;
    int i;

    msg = malloc(sizeof(struct i2c_msg) * num);
    if (!msg) {
        printf("create_msg_list error: malloc failed\n");
        return NULL;
    }
    memset(msg, 0x0, sizeof(struct i2c_msg) * num);
    for (i = 0; i < num; i++) {
        if ((i + 1) == num) {
            msg[i].next = NULL;
        } else {
            msg[i].next = &msg[i + 1];
        }
    }

    pdev->msgs = pdev->curr_msg = msg;

    return msg;
}

static void destory_msg_list(struct i2c_master_t *pdev)
{
    if (pdev->msgs) {
        free(pdev->msgs);
        pdev->msgs = pdev->curr_msg = NULL;
    }
}

/* Every time add one entry, return 1 if really added */
static int add_entry(struct i2c_master_t *pdev, struct i2c_msg *m)
{
    fifo_entry_t e;
    unsigned rnw_bit = (m->flags & I2C_FLAG_RD)?1:0;

    e.value = 0;

    if (m->state == I2C_MSG_STATE_READY) { /* First byte */
        m->state = I2C_MSG_STATE_START;
        if (m->flags & I2C_FLAG_NO_ADDR) {
            return 0;
        } else {
            e.s.icr_start = 1;
            e.s.data = (m->addr << 1) | rnw_bit;
            e.s.icr_tb = 1;
        }
    } else if (m->state == I2C_MSG_STATE_START) {
        if (rnw_bit) {
            e.s.data = 0;
        } else {
            e.s.data = m->buf[m->tx_idx];
        }
        m->tx_idx++;
        e.s.icr_tb = 1;
        if (m->tx_idx == m->len) { /* Last byte */
            if (rnw_bit) { /* Read */
                e.s.icr_acknak = 1; /* NAK */
            }
            if (m->flags & I2C_FLAG_STOP) {
                e.s.icr_stop = 1;
            }
            m->state = rnw_bit?I2C_MSG_STATE_RECEIVE:I2C_MSG_STATE_STOP;
        }
    } else {
        return 0;
    }
    //i2c_log_to_buf(pdev, "<====:0x%x\n", e.value);
    writel(e.value, pdev->master_reg + TWSI_WFIFO);
    return 1;
}

static int curr_msg_complete(struct i2c_master_t *pdev)
{
    struct i2c_msg *m = pdev->curr_msg;
    return (m->state == I2C_MSG_STATE_STOP)?1:0;
}

static int curr_msg_tx_complete(struct i2c_master_t *pdev)
{
    struct i2c_msg *m = pdev->curr_msg;
    return ((m->state == I2C_MSG_STATE_RECEIVE) || (m->state == I2C_MSG_STATE_STOP))?1:0;
}

static int all_msg_complete(struct i2c_master_t *pdev)
{
    struct i2c_msg *m = pdev->curr_msg;
    return (!m->next && (m->state == I2C_MSG_STATE_STOP))?1:0;
}

static int all_msg_tx_complete(struct i2c_master_t *pdev)
{
    struct i2c_msg *m = pdev->curr_msg;
    return (!m->next && ((m->state == I2C_MSG_STATE_RECEIVE) || (m->state == I2C_MSG_STATE_STOP)))?1:0;
}

/* return unused fifo entry remain number, only handle one msg */
static int _fill_tx_fifo(struct i2c_master_t *pdev, struct i2c_msg *msg, int fifo_remain)
{
    int buf_remain;
    int size, cnt;

    buf_remain = msg->len - msg->tx_idx;
    if ((msg->state == I2C_MSG_STATE_READY) && ((msg->flags & I2C_FLAG_NO_ADDR) == 0)) {
        buf_remain++; /* extra byte for address */
    }
    size = (fifo_remain < buf_remain)?fifo_remain:buf_remain;
    cnt = size;
    //printf("_fill_tx_fifo: fifo remain %d, size = %d\n", fifo_remain, size);
    while (cnt) {
        if (add_entry(pdev, msg))
            cnt--;
    }

    return (fifo_remain - size);
}

static int fill_tx_fifo(struct i2c_master_t *pdev)
{
    int tx_fifo_loop_cnt = 0;
    int fifo_remain = TX_FIFO_SIZE; /* TX FIFO is Empty */

    while (1) {
        if (tx_fifo_loop_cnt++ > TX_FIFO_LOOP_MAX) {
            i2c_log_to_buf(pdev, "tx loop err\n");
            return -1;
        }

        fifo_remain = _fill_tx_fifo(pdev, pdev->curr_msg, fifo_remain);
        if (all_msg_tx_complete(pdev) || (fifo_remain == 0)) {
            break;
        }

        if (curr_msg_tx_complete(pdev)) {
            pdev->curr_msg = pdev->curr_msg->next;
        }
    }

    return 0;
}

/* return unsed fifo entry number, only handle one msg */
static int _empty_rx_fifo(struct i2c_master_t *pdev, struct i2c_msg *msg, int fifo_avail)
{
    int size, cnt;
    unsigned data;
    int buf_remain = msg->len - msg->rx_idx;

    if (buf_remain < 0) {
        i2c_log_to_buf(pdev, "err:buf_remain=%d\n", buf_remain);
        return -1;
    }
    size = (fifo_avail < buf_remain)?fifo_avail:buf_remain;
    cnt = size;
    //if (log_out) printf("cnt %d\n", cnt);
    while (cnt--) {
        data = readl(pdev->master_reg + TWSI_RFIFO);
        msg->buf[msg->rx_idx++] = data & 0xFF;
        //i2c_log_to_buf(pdev, "====>:0x%x\n", data & 0xFF);
    }

    if (msg->rx_idx >= msg->len) {
        msg->state = I2C_MSG_STATE_STOP;
    }

    return (fifo_avail - size);
}

static int empty_rx_fifo(struct i2c_master_t *pdev, int fifo_avail)
{
    int rx_fifo_loop_cnt = 0;

    if (fifo_avail == 0) {
        fifo_avail = readl(pdev->master_reg + TWSI_RFIFO_WPTR) - readl(pdev->master_reg + TWSI_RFIFO_RPTR);
        if (fifo_avail < 0) { /* ptr is circular */
            fifo_avail += RFIFO_PTR_MAX;
        }
        if (fifo_avail > RX_FIFO_SIZE) {
            i2c_log_to_buf(pdev, "rx err %d\n", fifo_avail);
            goto error;
        }
    }

    while (1) {
        if (rx_fifo_loop_cnt++ > RX_FIFO_LOOP_MAX) {
            i2c_log_to_buf(pdev, "rx loop err\n");
            goto error;
        }
        //if (log_out) printf("empty_rx_fifo, fifo avail = %d\n", fifo_avail);
        fifo_avail = _empty_rx_fifo(pdev, pdev->curr_msg, fifo_avail);
        if (fifo_avail < 0) {
            goto error;
        }

        if (all_msg_complete(pdev) || (fifo_avail == 0)) {
            break;
        }

        if (curr_msg_complete(pdev)) {
            pdev->curr_msg = pdev->curr_msg->next;
        }
    }

    if (all_msg_complete(pdev) && fifo_avail) {
        i2c_log_to_buf(pdev, "all msgs completet but fifo not empty(%d)\n", fifo_avail);
        goto error;
    }

    return 0;
error:
    i2c_log_to_buf(pdev, "empty_rx_fifo err\n");
    return -1;
}

#define SR_ERR (SR_IBB | SR_SSD | SR_ALD | SR_GCAD | SR_SAD | SR_BED | SR_EBB | SR_RXOV)

static void i2c_irq_handle(void *arg)
{
    struct i2c_master_t *pdev;
    uint32_t isr = 0;

#if USE_SDK_API
    pdev = virq_to_pdev((unsigned)arg);
#else
    pdev = (struct i2c_master_t *)arg;
#endif
    if (!pdev) {
        goto error;
    }

    isr = readl(pdev->master_reg + TWSI_SR);

    //printf("isr = 0x%x, cr = 0x%x\n", (unsigned)isr, (unsigned)readl(pdev->master_reg + TWSI_CR));

    if (isr & SR_ERR) { /* i2c error */
        i2c_log_to_buf(pdev, "SR_ERR\n");
        goto error;
    } else { /* i2c ok, handle data */
        /* Handle rx before tx for avoid rx fifo overrun */
        if (isr & SR_RXHF) { /* Rx FIFO Half Full */
            //printf("int RXHF\n");
            if (empty_rx_fifo(pdev, RX_FIFO_SIZE / 2) < 0) {
                i2c_log_to_buf(pdev, "RXHF\n");
                goto error;
            }
        }

        if (isr & SR_TXDONE) { /* FIFO Transcation Done */
            //printf("int TXDONE\n");
            if (empty_rx_fifo(pdev, 0) < 0) {
                i2c_log_to_buf(pdev, "TXDONE\n");
                goto error;
            }
            fifo_xfer_enable(pdev, 0);
            /* wake up sender/receiver */
            i2c_xfer_done(pdev);
        } else if (isr & SR_TXE) { /* Tx FIFO Empty */
            //printf("int TXE\n");
            if (fill_tx_fifo(pdev) < 0) {
                i2c_log_to_buf(pdev, "TXE\n");
                goto error;
            }
        }
    }

    writel(isr, pdev->master_reg + TWSI_SR);

    return;
error:
    i2c_log_to_buf(pdev, "i2c irq err\n");
    pdev->error = isr;
    fifo_xfer_enable(pdev, 0);
    i2c_xfer_err(pdev);
    writel(isr, pdev->master_reg + TWSI_SR);

    return;
}

static int i2c_pio_xfer_irq(struct i2c_master_t *pdev)
{
    pdev->error = 0;
    i2c_xfer_ready(pdev);
    fifo_xfer_enable(pdev, 1);
    return 0;
}

void i2c_initialize(i2c_t dev)
{
    struct i2c_master_t *pdev;
    i2c_speed_t speed;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

#if USE_SDK_API
    uint8_t val;
    if (pdev->xfer_done == 0) {
        val = uos_create_flag();
        if (val == MAX_FLAG_NUM) {
            printf("create xfer done flag failed\n");
            goto error;
        }
        pdev->xfer_done = val;
    }

    if (pdev->mutex == 0) {
        val = uos_new_mutex();
        if (val == MAX_MUTEX_SIZE) {
            printf("create mutex failed\n");
            goto error;
        }
        pdev->mutex = val;
    }

    if (pdev->xfer_mutex == 0) {
        pdev->xfer_mutex = uos_new_mutex();
        if (pdev->xfer_mutex == MAX_MUTEX_SIZE) {
            printf("create xfer_mutex failed\n");
            goto error;
        }
    }
#endif

    if (!pdev->log_buf) {
        pdev->log_buf = malloc(I2C_LOG_BUF_LEN);
        if (!pdev->log_buf) {
            printf("malloc failed\n");
            goto error;
        }
    }

    speed = i2c_conf[dev].speed;
    pdev->speed = speed;

    if (pdev->pinmux_idx >= 0) {
        mfp_config(i2c_pinmux_cfgs[pdev->pinmux_idx]);
    }

    i2c_dev_reset(pdev);

    if (pdev->virq == 0) {
        if (pdev->irq != 0) {
            register_int_handler(pdev->irq, i2c_irq_handle, pdev);
            unmask_interrupt(pdev->irq);
        }
    }

#if USE_SDK_API
    extern unsigned int INTCGetIntVirtualNum(unsigned int IRQ_NO);
    if ((pdev->virq == 0) && (pdev->irq != 0)) {
        pdev->virq = INTCGetIntVirtualNum(pdev->irq);
    }
#else
    pdev->virq = pdev->irq;
#endif

    return;
error:
    printf("i2c_initialize failed\n");
    return;
}

int i2c_acquire(i2c_t dev)
{
    struct i2c_master_t *pdev;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        return -1;
    }

#if USE_SDK_API
    uos_take_mutex(pdev->mutex);
#endif

    return 0;
}

int i2c_release(i2c_t dev)
{
    struct i2c_master_t *pdev;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        return -1;
    }

#if USE_SDK_API
    uos_release_mutex(pdev->mutex);
#endif

    return 0;
}

int i2c_read_bytes(i2c_t dev, uint16_t address, void *data, size_t length, uint8_t flags)
{
    struct i2c_master_t *pdev;
    struct i2c_msg *p;
    int ret;

    (void)flags;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

    i2c_lock_bus(pdev);

    p = create_msg_list(pdev, 1);
    if (!p) {
        i2c_unlock_bus(pdev);
        goto error;
    }

    p->addr = address;
    p->buf = data;
    p->len = length;
    p->flags = I2C_FLAG_RD | I2C_FLAG_STOP;
    p->tx_idx = 0;
    p->rx_idx = 0;

    i2c_pio_xfer_irq(pdev);
    ret = i2c_wait_complete(pdev);
    destory_msg_list(pdev);

    i2c_unlock_bus(pdev);

    if (ret < 0) {
        goto error;
    }

    return 0;
error:
    return -1;
}

int i2c_read_regs(i2c_t dev, uint16_t address, uint16_t reg,
                  void *data, size_t length, uint8_t flags)
{
    struct i2c_master_t *pdev;
    struct i2c_msg *p;
    int ret;

    (void)flags;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

    i2c_lock_bus(pdev);

    p = create_msg_list(pdev, 2);
    if (!p) {
        i2c_unlock_bus(pdev);
        goto error;
    }

    p->addr = address;
    p->buf = (unsigned char *)&reg;
    p->len = 1;
    p->flags = I2C_FLAG_WR;
    p->tx_idx = 0;
    p->rx_idx = 0;

    p = p->next;
    p->addr = address;
    p->buf = data;
    p->len = length;
    p->flags = I2C_FLAG_RD | I2C_FLAG_STOP;
    p->tx_idx = 0;
    p->rx_idx = 0;

    i2c_pio_xfer_irq(pdev);
    ret = i2c_wait_complete(pdev);
    destory_msg_list(pdev);

    i2c_unlock_bus(pdev);

    if (ret < 0) {
        goto error;
    }

    return length;
error:
    return -1;
}

int i2c_write_bytes(i2c_t dev, uint16_t address, const void *data, size_t length, uint8_t flags)
{
    struct i2c_master_t *pdev;
    struct i2c_msg *p;
    int ret;

    (void)flags;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

    i2c_lock_bus(pdev);

    p = create_msg_list(pdev, 1);
    if (!p) {
        i2c_unlock_bus(pdev);
        goto error;
    }

    p->addr = address;
    p->buf = (void *)data;
    p->len = length;
    p->flags = I2C_FLAG_WR | I2C_FLAG_STOP;
    p->tx_idx = 0;
    p->rx_idx = 0;

    i2c_pio_xfer_irq(pdev);
    ret = i2c_wait_complete(pdev);
    destory_msg_list(pdev);

    i2c_unlock_bus(pdev);

    if (ret < 0) {
        goto error;
    }

    return length;
error:
    return -1;
}

int i2c_write_regs(i2c_t dev, uint16_t address, uint16_t reg,
                   const void *data, size_t length, uint8_t flags)
{
    struct i2c_master_t *pdev;
    struct i2c_msg *p;
    int ret;

    (void)flags;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

    i2c_lock_bus(pdev);

    p = create_msg_list(pdev, 2);
    if (!p) {
        i2c_unlock_bus(pdev);
        goto error;
    }

    p->addr = address;
    p->buf = (unsigned char *)&reg;
    p->len = 1;
    p->flags = I2C_FLAG_WR;
    p->tx_idx = 0;
    p->rx_idx = 0;

    p = p->next;
    p->addr = address;
    p->buf = (void *)data;
    p->len = length;
    p->flags = I2C_FLAG_WR | I2C_FLAG_NO_ADDR | I2C_FLAG_STOP;
    p->tx_idx = 0;
    p->rx_idx = 0;

    i2c_pio_xfer_irq(pdev);
    ret = i2c_wait_complete(pdev);
    destory_msg_list(pdev);

    i2c_unlock_bus(pdev);

    if (ret < 0) {
        goto error;
    }

    return length;
error:
    return -1;
}

int i2c_read_byte(i2c_t dev, uint16_t address, void *data, uint8_t flags)
{
    return i2c_read_bytes(dev, address, data, 1, flags);
}

int i2c_read_reg(i2c_t dev, uint16_t address, uint16_t reg, void *data, uint8_t flags)
{
    return i2c_read_regs(dev, address, reg, data, 1, flags);
}

int i2c_write_byte(i2c_t dev, uint16_t address, uint8_t data, uint8_t flags)
{
    return i2c_write_bytes(dev, address, &data, 1, flags);
}

int i2c_write_reg(i2c_t dev, uint16_t address, uint16_t reg, uint8_t data, uint8_t flags)
{
    return i2c_write_regs(dev, address, reg, &data, 1, flags);
}

static unsigned pi2c_dev_addr;

int arom_pi2c_init(unsigned addr)
{
    i2c_initialize(I2C_MASTER_PI2C);
    pi2c_dev_addr = addr;

    return 0;
}

int arom_pi2c_read_reg(unsigned reg, unsigned char *pval)
{
    if (i2c_read_reg(I2C_MASTER_PI2C, pi2c_dev_addr, reg, pval, 0) != 1) {
        printf("pi2c_read_reg failed\n");
        return -1;
    }

    return 0;
}

int arom_pi2c_write_reg(unsigned reg, unsigned char val)
{
    if (i2c_write_reg(I2C_MASTER_PI2C, pi2c_dev_addr, reg, val, 0) != 1) {
        printf("arom_pi2c_write_reg failed\n");
        return -1;
    }

    return 0;
}

int i2c_write_regs16(i2c_t dev, uint16_t address, uint16_t reg,
                   const void *data, size_t length, uint8_t flags)
{
    struct i2c_master_t *pdev;
    struct i2c_msg *p;
    int ret;
    int msg_cnt = 2;

    (void)flags;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

    i2c_lock_bus(pdev);

    if (data == NULL) {
        msg_cnt = 1;
    }

    p = create_msg_list(pdev, msg_cnt);
    if (!p) {
        i2c_unlock_bus(pdev);
        goto error;
    }

    uint8_t regs[2];
    regs[0] = (reg & 0xff);
    regs[1] = ((reg & 0xff00) >> 8);
    p->addr = address;
    p->buf = (unsigned char *)regs; //&reg;
    p->len = 2;
    if(data == NULL) {
        p->flags = I2C_FLAG_WR | I2C_FLAG_STOP;
    } else {
        p->flags = I2C_FLAG_WR;
    }
    p->tx_idx = 0;
    p->rx_idx = 0;

    if(data != NULL) {
        p = p->next;
        p->addr = address;
        p->buf = (void *)data;
        p->len = length;
        p->flags = I2C_FLAG_WR | I2C_FLAG_NO_ADDR | I2C_FLAG_STOP;
        p->tx_idx = 0;
        p->rx_idx = 0;
    }

    i2c_pio_xfer_irq(pdev);
    ret = i2c_wait_complete(pdev);
    destory_msg_list(pdev);

    i2c_unlock_bus(pdev);

    if (ret < 0) {
        goto error;
    }

    return length;
error:
    return -1;
}

int i2c_read_regs16(i2c_t dev, uint16_t address, uint16_t reg,
                  void *data, size_t length, uint8_t flags)
{
    struct i2c_master_t *pdev;
    struct i2c_msg *p;
    int ret;

    (void)flags;

    pdev = get_i2c_master(dev);
    if (!pdev) {
        goto error;
    }

    i2c_lock_bus(pdev);

    p = create_msg_list(pdev, 2);
    if (!p) {
        i2c_unlock_bus(pdev);
        goto error;
    }

    uint8_t regs[2];
    regs[0] = (reg & 0xff);
    regs[1] = ((reg & 0xff00) >> 8);

    p->addr = address;
    p->buf = (unsigned char *)regs;
    p->len = 2;
    p->flags = I2C_FLAG_WR;
    p->tx_idx = 0;
    p->rx_idx = 0;

    p = p->next;
    p->addr = address;
    p->buf = data;
    p->len = length;
    p->flags = I2C_FLAG_RD | I2C_FLAG_STOP;
    p->tx_idx = 0;
    p->rx_idx = 0;

    i2c_pio_xfer_irq(pdev);
    ret = i2c_wait_complete(pdev);
    destory_msg_list(pdev);

    i2c_unlock_bus(pdev);

    if (ret < 0) {
        goto error;
    }

    return length;
error:
    return -1;
}

#endif

